#include "aim_dec/Armordecision.hpp"

int PnPdecision::Armor_decision(const std::vector<aim_deps::Armor> &Input_armors, int spec, int exc)
{
     _input_armors = Input_armors;
#ifdef _DEBUG_DECISION
     Draw_and_output(Input_armors, Input_mat);
#endif
     _all_evarmors_score.clear();
     for (auto &armor : Input_armors)
     {
          aim_deps::Evaluated_armor _eva_armor;
          //calculate the scores
          if (spec != 0 && armor.armor_number == spec)
          {
               if (armor.valid)
               {
                    _eva_armor.Distance_score = armor.t_vec.z * _params->Distant_base;
                    _eva_armor.Rotation_score = (armor.t_vec.y + armor.t_vec.x) * _params->Size_base;
                    _eva_armor.Total_score = _eva_armor.Distance_score+_eva_armor.Rotation_score;
                    _eva_armor._armor = armor;
                    _all_evarmors_score.emplace_back(_eva_armor.Total_score);
                    //std::cout <<_params->Distance_multi << std::endl;
               }
               else{
                    _all_evarmors_score.emplace_back(aim_deps::FAILED_SCORE); //invalid装甲板分值为1023
               }
          }
     }

     //get the index of the armor which has the highest totalscore
     auto maxPosition = max_element(_all_evarmors_score.begin(), _all_evarmors_score.end());
     return maxPosition - _all_evarmors_score.begin();
}

/// 简化版的自由决策（取距离最近的）
int PnPdecision::Armor_decision(const std::vector<aim_deps::Armor> &Input_armors) const{
     //std::cout<<"free target"<<std::endl;
     float mini = 1e12;
     int mini_pos = 0;
     for(size_t i = 0; i < Input_armors.size(); i++) {
          float distance = Input_armors[i].t_vec.z;
          float x_distance = std::abs(Input_armors[i].t_vec.x);
          if(Input_armors[i].valid && distance > 100.0) {
               if (x_distance < mini) {
                    mini_pos = int(i);
                    mini = x_distance;
               }
          }
     }
     return mini_pos;
}